/**
 *@note Copyright 2012-2022 CETHIK All Rights Reserved.
 *@brief 代价地图主类文件
 *@author tuyinan
 *@data 2022-04-xx
 */
#ifndef COSTMAP_2D_LAYERED_COSTMAP_H_
#define COSTMAP_2D_LAYERED_COSTMAP_H_

#include <costmap_2d/cost_values.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/layer.h>

#include <string>
#include <vector>

namespace costmap_2d {
class Layer;

/**
 *@brief 主代价地图类
 *@author tuyinan
 *@version 2022-04-xx
 */
class LayeredCostmap {
 public:
  /**
   *@brief 构造函数
   *@param std::string global_frame 全局参照
   *@param bool rolling_window 是否采用滚动窗口
   *@param bool track_unknown 是否检查未知区域
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  LayeredCostmap(std::string global_frame, bool rolling_window,
                 bool track_unknown);

  /**
   *@brief 析构函数
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  ~LayeredCostmap();

  /**
   *@brief 独立的更新地图方法
   *@param double robot_x 机器人位姿x
   *@param double robot_y 机器人位姿y
   *@param double robot_yaw 机器人角度
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void updateMap(double robot_x, double robot_y, double robot_yaw);

/**
*@brief 获取全局参照
*@param 无
*@return std::string
*@author: tuyinan
*@version: 2022-04-xx
*/
  std::string getGlobalFrameID() const { return global_frame_; }
/**
*@brief resize 地图
*@param unsigned int size_x 宽
*@param unsigned int size_y 高
*@param double resolution 转换映射
*@param double origin_x 地图起始点x
*@param double origin_y 地图起始点y
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  void resizeMap(unsigned int size_x, unsigned int size_y, double resolution,
                 double origin_x, double origin_y);

/**
*@brief 获取更新后的边界
*@param double& minx 最小的x
*@param double& miny 最小的y
*@param double& maxx 最大的x
*@param double& maxy 最大的y
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  void getUpdatedBounds(double& minx, double& miny, double& maxx,
                        double& maxy) {
    minx = minx_;
    miny = miny_;
    maxx = maxx_;
    maxy = maxy_;
  }
/**
*@brief 是否数据是新的
*@param 无
*@return bool 是或否
*@author: tuyinan
*@version: 2022-04-xx
*/
  bool isCurrent();
/**
*@brief 获取代价地图
*@param 无
*@return Costmap2D* 地图
*@author: tuyinan
*@version: 2022-04-xx
*/
  Costmap2D* getCostmap() { return &costmap_; }
/**
*@brief 是否采用滚动窗口
*@param 无
*@return boo 是或否
*@author: tuyinan
*@version: 2022-04-xx
*/
  bool isRolling() { return rolling_window_; }
/**
*@brief 判断是否默认未知区域为没信息
*@param 无
*@return bool 是或否
*@author: tuyinan
*@version: 2022-04-xx
*/
  bool isTrackingUnknown() {
    return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION;
  }
/**
*@brief 获取所有的层
*@param 无
*@return std::vector<boost::shared_ptr<Layer> >* 层级集合
*@author: tuyinan
*@version: 2022-04-xx
*/
  std::vector<boost::shared_ptr<Layer> >* getPlugins() { return &plugins_; }
/**
*@brief 添加层级
*@param boost::shared_ptr<Layer> plugin 层
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  void addPlugin(boost::shared_ptr<Layer> plugin) {
    plugins_.push_back(plugin);
  }
/**
*@brief 获取边界
*@param unsigned int* x0 边界小点x
*@param unsigned int* y0 边界小点y
*@param unsigned int* xn 边界大点x
*@param unsigned int* yn 边界大点y
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0,
                 unsigned int* yn) {
    *x0 = bx0_;
    *xn = bxn_;
    *y0 = by0_;
    *yn = byn_;
  }
/**
*@brief 是否初始化过
*@param 无
*@return bool 是或否
*@author: tuyinan
*@version: 2022-04-xx
*/
  bool isInitialized() { return initialized_; }

/**
*@brief 设置占位
*@param const std::vector<costmap_2d::Point>& footprint_spec 占位
*@return void
*@author: tuyinan
*@version: 2022-04-xx
*/
  void setFootprint(const std::vector<costmap_2d::Point>& footprint_spec);

/**
*@brief 获取占位
*@param 无
*@return const std::vector<costmap_2d::Point>& 占位
*@author: tuyinan
*@version: 2022-04-xx
*/
  const std::vector<costmap_2d::Point>& getFootprint() { return footprint_; }

/**
*@brief 获取最大半径
*@param 无
*@return double 半径
*@author: tuyinan
*@version: 2022-04-xx
*/
  double getCircumscribedRadius() { return circumscribed_radius_; }

/**
*@brief 获取最小半径
*@param 无
*@return double 半径
*@author: tuyinan
*@version: 2022-04-xx
*/
  double getInscribedRadius() { return inscribed_radius_; }

 private:
 /// 地图
  Costmap2D costmap_;
  /// 全局参照
  std::string global_frame_;
/// 是否采用滚动窗口
  bool rolling_window_;  /// < @brief Whether or not the costmap should roll
                         /// with the robot
/// 是否数据是最新的
  bool current_;
/// 更新后的边界
  double minx_, miny_, maxx_, maxy_;
/// 边界
  unsigned int bx0_, bxn_, by0_, byn_;
/// 层集合
  std::vector<boost::shared_ptr<Layer> > plugins_;
/// 是否初始化
  bool initialized_;
/// 外半径和内半径
  double circumscribed_radius_, inscribed_radius_;
  /// 机器人占位
  std::vector<costmap_2d::Point> footprint_;
};

}  // namespace costmap_2d

#endif  // COSTMAP_2D_LAYERED_COSTMAP_H_
